#include "New_Pid.h"
 
void VALUE_LIMIT(float Input,float Min,float Max)
{
	if(Input>Max)
		Input=Max;
	else if(Input<Min)
		Input=Min;
	else
		Input=Input;
}

void NewPid_init(NewPid_Info_Typedef *Pid,NewPid_Type_e Type,float Param[5])
{
	Pid->Kp=Param[0];
	Pid->Ki=Param[1];
	Pid->Kd=Param[2];
	Pid->Integral_Limit=Param[3];
	Pid->Limit=Param[4];
	
	Pid>Type=Type;
}

void NewPid_Controller(NewPid_Info_Typedef *Pid,float Target,float Masure)
{
	if(Pid->Type==POSITION)
	{
	Pid->Error[0]=Target-Masure;
	Pid->Error[1]=Pid->Error[0];
	
	Pid->Integral+=Pid->Error[0];
	VALUE_LIMIT (Pid->Integral,-Pid->Integral_Limit,Pid->Integral_Limit);
	
	Pid->Output=Pid->Kp*Pid->Error[0]+Pid->Integral*Pid->Ki+Pid->Kd*(Pid->Error[0]-Pid->Error[1]);
	VALUE_LIMIT (Pid->Output,-Pid->Limit,Pid->Limit);
	}
	else if(Pid->Type==VELOCITY)
	{
		Pid->Error[2]=Pid->Error[1];
		Pid->Error[1]=Pid->Error[0];
		Pid->Error[0]=Target-Masure;
		
		Pid->Integral+=Pid->Error[0];
		VALUE_LIMIT (Pid->Integral,-Pid->Integral_Limit,Pid->Integral_Limit);
		
		Pid->Output=Pid->Kp*(Pid->Error[0]-Pid->Error[1])-Pid->Ki*Pid->Error[0]+Pid->Kd*(Pid->Error[0]-2*Pid->Error[1]+Pid->Error[2]);
	VALUE_LIMIT (Pid->Output,-Pid->Limit,Pid->Limit);
	}
}